#include <kernel.h>

#define USE_HEAP 1

volatile const uint32_t *hpet_tick;
uint32_t                 hpet_tick_freq;

static proc_t hpet_timer_proc;

static void(*set_hpet_timer)(uint32_t);

#define HPET_TIMER_SIZE  1024
#define HPET_TIMER_LEVEL 20
static struct hpet_timer_s
{
	 union
	 {
		  struct
		  {
			   hpet_timer_cb_f cb;
			   void      *data;
			   int        status;
#if USE_SKIP_LIST
			   int             level;
			   struct hpet_timer_s *next[HPET_TIMER_LEVEL];
			   struct hpet_timer_s  close_next;
#elif USE_HEAP
			   int heap_idx;
			   int close_next;
#endif
			   uint32_t   tp;
		  };
		  int free_next;
	 };	 
} hpet_timers[HPET_TIMER_SIZE];

#define HPET_TIMER_STATUS_FREE   0
#define HPET_TIMER_STATUS_OPEN   1
#define HPET_TIMER_STATUS_CLOSED 2

static int hpet_timer_count;
static int free_head;

#if USE_SKIP_LIST
struct hpet_timer_s *hpet_timer_head[HPET_TIMER_LEVEL];
#elif USE_HEAP
static int hpet_timer_heap[HPET_TIMER_SIZE + 1];
static spin_lock_t hpet_timer_heap_lock;
#endif

static uint32_t old_counter;
static spin_lock_t hpet_timer_alloc_lock;


/* HPET ======================================================= */

#define HR_GEN_CAP_ID 0
#define HR_GEN_CONF   2
#define HR_IRQ_STATUS 4
#define HR_MAIN_COUNTER_VALUE 30

#define HRT_BASE(n)    ((n << 2) + 32)
#define HRT_CONF(n)    (HRT_BASE(n) + 0)
#define HRT_COMP(n)    (HRT_BASE(n) + 1)
#define HRT_FSB_INT(n) (HRT_BASE(n) + 2)

static volatile uint64_t *hpet_regs;
static uint32_t hpet_period;
static int      hpet_hpet_timer;
static volatile uint32_t *hpet_counter;

#define TP_AFTER_NS(ns) ((uint32_t)(*hpet_tick + (uint32_t)(0.000000001 * hpet_tick_freq * ns)))
#define TP_AFTER_US(us) ((uint32_t)(*hpet_tick + (uint32_t)(0.000001 * hpet_tick_freq * us)))
#define TP_AFTER_MS(ms) ((uint32_t)(*hpet_tick + (uint32_t)(0.001 * hpet_tick_freq * ms)))

static void
hpet_set_hpet_timer(uint32_t tp)
{
	 hpet_regs[HRT_COMP(hpet_hpet_timer)] = tp;
}

static int
hpet_init(void)
{
	 hpet_regs = (volatile uint64_t *)ekf_hpet;

	 uint32_t id_hi = hpet_regs[HR_GEN_CAP_ID] >> 32;
	 uint32_t id_lo = hpet_regs[HR_GEN_CAP_ID];

	 ekf_kprintf("HPET INFO[%08x %08x]:\n\trev_id 0x%02x hpet_timers: %d width: %d period: %d (10^-15 s)\n",
			 id_hi, id_lo,
			 id_lo & 0xff, (id_lo >> 8) & 0xf, (id_lo & (1 << 13)) ? 64 : 32, id_hi);
	 // hpet_counter_width = (id_lo & (1 << 13)) ? 64 : 32;
	 hpet_period = id_hi;
	 hpet_hpet_timer = (id_lo >> 8) & 0xf;

	 /* Set the configuration to enable CNF and disable legacy replacement */
	 hpet_regs[HR_GEN_CONF] = 1;

	 uint64_t id = hpet_regs[HRT_CONF(hpet_hpet_timer)];
	 /* disable fsb */
	 id &= ~((uint64_t)1 << 14);
	 /* force 32-bit mode */
	 if (id & ((uint64_t)1 << 4))
		  id &= ~((uint64_t)1 << 3);
	 /* enable the interrupt 2, edge triggered */
	 id &= ~((uint64_t)0x1f << 9);
	 id |=  ((uint64_t)2 << 9);
	 
	 id |= ((uint64_t)1 << 2);
	 id &= ~((uint64_t)1 << 1);
	 hpet_regs[HRT_CONF(hpet_hpet_timer)] = id;
	 hpet_counter = (uint32_t *)(hpet_regs + HR_MAIN_COUNTER_VALUE);
	
	 /* Listen on irq 2 */
	 hpet_tick = hpet_counter;
	 hpet_tick_freq = 1000000000000000.0 / hpet_period;
	 set_hpet_timer = &hpet_set_hpet_timer;

	 ekf_kprintf("HPET FREQ = %d\n", hpet_tick_freq);
	 
	 return 0;
}

/* Abstract =================================================== */

int
hpet_timer_open(hpet_timer_cb_f cb, void *data, uint32_t tp)
{
	 /* if ((int32_t)(tp - *hpet_tick) / (double)hpet_tick_freq < */
	 /* 	 0.0001) */
	 /* { */
	 /* 	  cb(data); */
	 /* 	  return -1; */
	 /* } */

	 intr_irq_save();
	 spl_acquire(&hpet_timer_alloc_lock);
	 
	 int result = free_head;
	 if (result != -1)
		  free_head = hpet_timers[result].free_next;
	 
	 spl_release(&hpet_timer_alloc_lock);

	 if (result == -1) return -1;
	 hpet_timers[result].status = HPET_TIMER_STATUS_OPEN;

	 spl_acquire(&hpet_timer_heap_lock);

	 hpet_timers[result].heap_idx = ++ hpet_timer_count;
	 hpet_timers[result].cb       = cb;
	 hpet_timers[result].data     = data;
	 hpet_timers[result].tp       = tp;
	 
	 int cur = hpet_timers[result].heap_idx;
	 while (cur > 1)
	 {
		  int comp = cur >> 1;
		  if ((int32_t)(hpet_timers[hpet_timer_heap[comp]].tp - tp) > 0)
		  {
			   hpet_timer_heap[cur] = hpet_timer_heap[comp];
			   hpet_timers[hpet_timer_heap[cur]].heap_idx = cur;
			   
			   cur = comp;
		  }
		  else break;
	 }

	 hpet_timer_heap[cur] = result;
	 hpet_timers[result].heap_idx = cur;

	 /* ekf_kprintf("hpet_timer %d (%08x %08x) open %08x %d %08x %08x\n", */
	 /* 		 result, cb, data, tp, */
	 /* 		 hpet_timer_heap[1], hpet_timers[hpet_timer_heap[1]].tp, *hpet_tick); */
	 set_hpet_timer(hpet_timers[hpet_timer_heap[1]].tp);
	 if ((hpet_timers[hpet_timer_heap[1]].tp - *hpet_tick) > 0x80000000)
	 {
		  proc_legacy_notify(hpet_timer_proc);
	 }

	 spl_release(&hpet_timer_heap_lock);
	 intr_irq_restore();

	 return result;
}

int
hpet_timer_open_ns(hpet_timer_cb_f cb, void *data, uint32_t ns)
{
	 return hpet_timer_open(cb, data, TP_AFTER_NS(ns));
}

int
hpet_timer_open_us(hpet_timer_cb_f cb, void *data, uint32_t us)
{
	 return hpet_timer_open(cb, data, TP_AFTER_US(us));
}

int
hpet_timer_open_ms(hpet_timer_cb_f cb, void *data, uint32_t ms)
{
	 return hpet_timer_open(cb, data, TP_AFTER_MS(ms));
}

void
hpet_timer_close_unsafe(int idx)
{
	 int cur = hpet_timers[idx].heap_idx;
	 int rep = hpet_timer_heap[cur] = hpet_timer_heap[hpet_timer_count --];
	 uint32_t tp = hpet_timers[idx].tp;

	 hpet_timers[idx].status = HPET_TIMER_STATUS_CLOSED;
	 if (hpet_timer_count == 0) return;

	 while (1)
	 {
		  int comp = cur << 1;
		  if (comp > hpet_timer_count) break;
		  if (comp < hpet_timer_count &&
			  (int32_t)(hpet_timers[hpet_timer_heap[comp]].tp - hpet_timers[hpet_timer_heap[comp + 1]].tp) > 0)
			   ++ comp;
		  
		  if ((int32_t)(hpet_timers[hpet_timer_heap[comp]].tp - tp) < 0)
		  {
			   hpet_timer_heap[cur] = hpet_timer_heap[comp];
			   hpet_timers[hpet_timer_heap[cur]].heap_idx = cur;
			   
			   cur = comp;
		  }
		  else break;
	 }

	 hpet_timer_heap[cur] = rep;
	 hpet_timers[rep].heap_idx = cur;
}

void
hpet_timer_close(int idx)
{
	 if (idx == -1) return;
	 intr_irq_save();
	 spl_acquire(&hpet_timer_heap_lock);
	 
	 if (hpet_timers[idx].status == HPET_TIMER_STATUS_OPEN)
		  hpet_timer_close_unsafe(idx);
	 
	 spl_release(&hpet_timer_heap_lock);
	 intr_irq_restore();
}

int
hpet_timer_test(int idx)
{
	 if (idx == -1) return 0;
	 return hpet_timers[idx].status == HPET_TIMER_STATUS_OPEN;
}

int
hpet_timer_free(int idx)
{
	 if (idx == -1) return 0;
	 if (hpet_timers[idx].status == HPET_TIMER_STATUS_CLOSED)
	 {
		  intr_irq_save();
		  spl_acquire(&hpet_timer_alloc_lock);
		  
		  hpet_timers[idx].status = HPET_TIMER_STATUS_FREE;
		  hpet_timers[idx].free_next = free_head;
		  free_head = idx;
		  
		  spl_release(&hpet_timer_alloc_lock);
		  intr_irq_restore();

		  return 0;
	 }
	 else return 1;
}

static int
hpet_timer_event(void)
{
	 // ekf_kprintf("hpet_timer event start\n");
	 int wait;
	 int close_head;
	 
	 wait = 1;
	 close_head = -1;

	 intr_irq_save();
	 spl_acquire(&hpet_timer_heap_lock);

	 while (hpet_timer_count > 0 &&
			(hpet_timers[hpet_timer_heap[1]].tp - *hpet_tick) > 0x80000000)
	 {
		  int idx = hpet_timer_heap[1];
		  hpet_timer_close_unsafe(idx);
		  hpet_timers[idx].close_next = close_head;
		  close_head = idx;
	 }

	 if (hpet_timer_count > 0)
	 {
		  set_hpet_timer(hpet_timers[hpet_timer_heap[1]].tp);
		  if ((hpet_timers[hpet_timer_heap[1]].tp - (old_counter = *hpet_tick)) > 0x80000000)
			   wait = 0;
	 }
	 spl_release(&hpet_timer_heap_lock);
	 intr_irq_restore();
	 
	 while (close_head != -1)
	 {
	 	  /* ekf_kprintf("hpet_timer %d closed, cb = %08x, data = %08x\n", */
	 	  /* 		  close_head, hpet_timers[close_head].cb, hpet_timers[close_head].data); */
		  if (hpet_timers[close_head].status == HPET_TIMER_STATUS_CLOSED)
		  {
			   /* ekf_kprintf("hpet_timer %d closed, cb = %08x, data = %08x\n", */
			   /* 		   close_head, hpet_timers[close_head].cb, hpet_timers[close_head].data); */
			   hpet_timers[close_head].cb(hpet_timers[close_head].data);
			   /* ekf_kprintf("callback finished\n"); */
		  }
	 	  close_head = hpet_timers[close_head].close_next;
	 }
	 // ekf_kprintf("hpet_timer event end, wait = %d\n", wait);
	 return wait;
}

static void
__hpet_timer_proc(void *_arg)
{
	 while (1)
	 {
		  if (hpet_timer_event())
			   proc_legacy_wait_try();
	 }
}

static void
__hpet_irq_handler(int irq, int c)
{
	 proc_legacy_notify(hpet_timer_proc);
	 ekf_intr_irq_eoi(irq);
}

int 
hpet_timer_init(void)
{
	 hpet_init();

	 spl_init(&hpet_timer_heap_lock);
	 spl_init(&hpet_timer_alloc_lock);
   
	 hpet_timer_count = 0;
	 int i;
	 for (i = 0; i != HPET_TIMER_SIZE; ++ i)
	 {
	 	  hpet_timers[i].free_next = i + 1;
	 	  hpet_timers[i].status = HPET_TIMER_STATUS_FREE;
	 }
	 hpet_timers[HPET_TIMER_SIZE - 1].free_next = -1;
	 free_head = 0;
	 
	 // proc_attach(hpet_timer_proc = proc_create("__hpet_timer", 0, proc_kvpt_get(), __hpet_timer_proc, NULL));
	 // intr_irq_handler_set(2, &__hpet_irq_handler, 1);
	 // intr_irq_handler_set(15, &__hpet_irq_handler, 1);
	 // ekf_intr_irq_line_enable(2);
	 // ekf_intr_irq_line_enable(15);

	 return 0;
}
